import traci
from numpy import sin,cos
from scipy.optimize import fsolve



class controlModel(object):
    def __init__(self, carID):
       self.carID = carID #self的意思是实例对象的本身
       self.type = traci.vehicle.getTypeID(carID) #车辆的类型
       self.speed =traci.vehicle.getSpeed(self.carID) #获得车该时刻车辆的速度
       self.acc = traci.vehicle.getAcceleration(self.carID) #获得该时刻车辆的加速度
       self.lanePosition = traci.vehicle.getLanePosition(self.carID) #获得车辆在车道上的位置（前保险杠到车道起点）
       self.length = traci.vehicle.getLength(self.carID) #获得车辆的车身长度，模型中的L
       self.lane = traci.vehicle.getLaneID(self.carID) #获得车辆行驶时候的车道在哪
       self.maxacc = traci.vehicle.getAccel(self.carID) #获得车辆最大加速度的可能性
       self.maxdec = traci.vehicle.getDecel(self.carID) #获得车辆的最大减速度的可能性
       self.distance = traci.vehicle.getDistance(self.carID) #获得车辆行驶过的距离
       self.buslen = 10 #网联公交车的车身长度
       self.carlen = 5 #小汽车的车身长度
       self.minheadway = 3.48 #最小车头时距
       self.minGap = 1.5 #前后车辆最小间距
       self.time = traci.simulation.getTime() 
       self.CO2 = traci.vehicle.getCO2Emission(self.carID) #车辆在该时间步的CO2排放量，用来绘图

       
#获得相邻车道前车的ID,获得LV的ID
    def frontCar(self):
        m=200
        LV_ID =""
        for carID in traci.vehicle.getIDList():#对所有的车辆进行遍历
            lanePosition = traci.vehicle.getLanePosition(carID) #获得每个车辆在车道上面的位置
            if self.lane == "genE1_1" and traci.vehicle.getLaneID(carID) =="genE1_0" and 200 > lanePosition > self.lanePosition:
               if lanePosition - self.lanePosition< m:
                  m = lanePosition - self.lanePosition
                  LV_ID = carID
        return LV_ID
   
#获得相邻车道后车的ID，获得FV的ID
    def rearCar(self):
        m=200
        FV_ID=""
        for carID in traci.vehicle.getIDList():#对所有的车辆进行遍历
            lanePosition = traci.vehicle.getLanePosition(carID)
            if self.lane ==  "genE1_1" and traci.vehicle.getLaneID(carID) == "genE1_0" and lanePosition < self.lanePosition:      
               if self.lanePosition - lanePosition< m:            
                  m = self.lanePosition - lanePosition
                  FV_ID = carID
        return FV_ID

    #求解车辆FV速度的相关控制参数
    def calculate_parameters_for_FV(FV_ID, equation_list, condition_list,
                                    initial_guess_list=None, default_speed=None,
                                    condition_1=None,condition_2=None,condition_3=None,
                                    condition_4=None,condition_5=None,condition_6=None,
                                    condition_7=None,condition_8=None,condition_9=None):
        for equation,condition_function,initial_guess in zip(equation_list,condition_list,initial_guess_list):
            x = fsolve(equation,x0 = initial_guess)
            if condition_function(x):
               if condition_function == condition_1:
                   speed_next = speed_generate_1(x)
               elif condition_function == condition_2:
                   speed_next = speed_generate_2(x)
               elif condition_function == condition_3:
                   speed_next = speed_generate_3(x)
               elif condition_function == condition_4:
                   speed_next = speed_generate_4(x)
               elif condition_function == condition_5:
                   speed_next = speed_generate_5(x)
               elif condition_function == condition_6:
                   speed_next = speed_generate_6(x)
               elif condition_function == condition_7:
                   speed_next = speed_generate_7(x)
               elif condition_function == condition_8:
                   speed_next = speed_generate_8(x)
               elif condition_function == condition_9:
                   speed_next = speed_generate_9(x)
               return speed_next

        return default_speed


























    #求解控制序列为a_min、p、a_max时候的相关参数
    #t_1 = x[4]、t_2 = x[5]、t_f = x[6]、c_1 = x[7]、c_2 = x[8]
    def equation_1(x):
        eqs = []
        #中间变量t1时刻的速度和位置
        eqs.append(x[0] - 12 + 4 * x[4])
        eqs.append(x[1] - 30 - 12 * x[4] + 0.5 * 4 * x[4] * x[4])

        eqs.append({x[2] - x[0] - 0.5 * x[7] * (x[4] * x[4] - x[5] * x[5]) + x[8] * (x[4] - x[5])})
        eqs.append(x[3] - x[1] - 0.15 * x[7] * (x[4] * x[4] * x[4] - x[5] * x[5] * x[5])
                   - 0.5 * x[7] * (x[4] * x[4] - x[5] * x[5]) - (0.5 * x[7] * x[4] * x[4] * (x[5] - x[4])))

        eqs.append(x[2] + 3 * (x[6] - x[5]) - 12 * (1 - 0.15 * sin(0.2 * x[6])))

        eqs.append(x[3] + x[2] * (x[6] - x[5]) + 0.5 * 3 * (x[6] * x[6]) + 20 + 2 * 1.8 * (x[2] + 3 * (x[6] - x[5])) - (100 + 12 * x[6] + 10 * cos(0.2 * x[6]) - 10))
        eqs.append(0.5 * 9 + 0.5 * 25 - (-x[7] * x[6] + x[8]) * 3 - (2 * 1.8 * x[7] + x[7] * x[6] - x[8]) * (-10 * cos(0.2 * x[6])))

        eqs.append(-x[7] * x[4] + x[8] - 3)
        eqs.append(-x[7] * x[5] + x[8] + 4)

        t_1 = x[4],t_2 = x[5],t_f = x[6],c_1 = x[7],c_2 = x[8]
        return t_1,t_2,t_f,c_1,c_2



    #求解控制序列为p、a_max时候的相关参数
    #t_1 = x[2]、t_f = x[3]、c_1 = x[4]、c_2 = x[5]
    def equation_2(x):
        eqs=[]
        #中间变量t1时刻的速度与位移
        eqs.append(x[0]-12+0.5*x[4]-x[5]*x[2])
        eqs.append(x[1]-30+0.15*x[4]*x[2]*x[2]*x[2]-0.5*x[5]*x[2]*x[4]-30*x[2])
        # 末态tf时刻的速度和位置
        eqs.append(x[0]+3*(x[3]-x[2])-12 * (1 - 0.15 * sin(0.2 * x[3])))
        eqs.append(x[1]+x[0]*(x[3]-x[2])+0.5*3*(x[3]-x[2])*(x[3]-x[2])+20+2*1.8*12*(1-0.15*sin(0.2*x[3]))-(100+12*x[3]+10*cos(0.2 * x[3]) - 10))
        #横截条件
        eqs.append(0.5 * 9 + 0.5 * 25 - (-x[4] * x[3] + x[5]) * 3 - (2 * 1.8 * x[4] + x[4] * x[3] - x[5]) * (-10 * cos(0.2 * x[3])))
        #协作变量
        eqs.append(-x[4]*x[2]+x[5]+4)

        t_1 = x[2],t_f = x[3],c_1 = x[4],c_2 = x[5]
        return t_1,t_f,c_1,c_2



    #求解控制序列为a_max、p、a_min时候的相关参数
    #t_1 = x[4]、t_2 = x[5]、t_f = x[6]、c_1 = x[7]、c_2 = x[8]
    def equation_3(x):
        eqs=[]
        #中间变量t1时刻的速度和位置
        eqs.append(x[0]-12-3*x[4])
        eqs.append(x[1]-30-12*x[4]-0.5*3*x[4]*x[4])
        #中间变量t2时刻的速度和位置
        eqs.append(x[2]-x[0]-0.5*x[7]*(x[4]*x[4]-x[5]*x[5])-x[8]*(x[4]-x[5]))
        eqs.append(x[3]-x[1] - 0.15*x[7]*(x[4]*x[4]*x[4]-x[5]*x[5]*x[5])-0.5*x[8]*(x[4]*x[4]-x[5]*x[5])-(0.5*x[7]*x[4]*x[4]-x[8]*x[4])*(x[5]-x[4]))
        #末态tf时刻的速度和位置
        eqs.append(x[2]-4*(x[6]-x[5])-12 * (1 - 0.15 * sin(0.2 * x[6])))
        eqs.append(x[3]+x[2]*(x[6]-x[5])-0.5*4*(x[6]-x[5])*(x[6]-x[5])+20+2*1.8*(1 - 0.15 * sin(0.2 * x[6]))-(100+12*x[6]+10*cos(0.2 * x[6]) - 10))
        #横截条件
        eqs.append(-0.5*4*4+0.5*25+(-x[7]*x[6]+x[8])*4-(2*1.8*x[7]+x[7]*x[6]-x[8])*(-10 * cos(0.2 * x[7])))

        t_1 = x[4],t_2 = x[5],t_f = x[6],c_1 = x[7],c_2 = x[8]
        return t_1,t_2,t_f,c_1,c_2



    #求解控制序列为a_min、p时候的相关参数
    # t_1 = x[2]、t_f = x[3]、c_1 = x[4]、c_2 = x[5]
    def equation_4(x):
        eqs=[]
        #中间变量t1时刻的速度和位置
        eqs.append(x[0] - 30 + 4*x[2])
        eqs.append(x[1] - 30 - 12*x[2] + 0.5*4*x[2]*x[2])
        #末态tf时刻的速度和位置
        eqs.append(x[0]+0.5*x[4]*(x[2]*x[2]-x[3]*x[3])+x[5]*(x[3]-x[2])-12*(1 - 0.15 * sin(0.2 * x[6])))
        eqs.append(x[1]+0.15*x[4]*(x[2]*x[2]*x[2]-x[3]*x[3]*x[3])+0.5*x[5]*(x[3]*x[3]-x[2]*x[2])+(0.5*x[2]*x[2]-x[5]*x[2])*(x[3]-x[2])-(2*10-2*1.8*12*(1 - 0.15 * sin(0.2 * x[3]))+(100 + 12 * x[3] + 10 * cos(0.2 * x[3]) - 10)))
        #横截条件
        eqs.append(0.5*(-x[4]*x[3]+x[5])*(-x[4]*x[3]+x[5])+0.5*25-(-x[4]*x[3]+x[5])*(-x[4]*x[3]+x[5])-(2*1.8*x[4]+x[4]*x[3]-x[5])*(-10 * cos(0.2 * x[3])))
        #协作变量
        eqs.append(-x[4]*x[2]+x[5]-3)

        t_1 = x[2],t_f = x[3],c_1 = x[4],c_2 = x[5]
        return t_1,t_f,c_1,c_2



    #求解控制序列为a_max的相关参数
    #t_f = x[0]
    def equation_5(x):
        eqs=[]
        #末态tf时刻的速度和位置
        eqs.append(12+3*x[0]-12*(1 - 0.15 * sin(0.2 * x[0])))
        eqs.append(30+12*x[0]+0.5*3*x[0]*x[0]-(2*10-2*1.8*12*(1 - 0.15 * sin(0.2 * x[0]))+(100 + 12 * x[0] + 10 * cos(0.2 * x[0]) - 10)))

        t_1 = x[0]
        return t_1



    #求解控制序列为p、a_min时候的相关参数
    #t_1 = x[2]、t_f = x[3]、c_1 = x[4]、c_2 = x[5]
    def equation_6(x):
        eqs=[]
        # 中间变量t1时刻的速度和位置
        eqs.append(x[0]-12+0.5*x[4]*x[2]*x[2]-x[5]*x[2])
        eqs.append(x[1]-30+0.15*x[4]*x[2]*x[2]*x[2]-0.5*x[5]*x[2]*x[2]-12*x+[2])
        #末态tf时刻的速度和位置
        eqs.append(x[0]-4*(x[3]-x[2])-12*(1-0.15*sin(0.2*x[3])))
        eqs.append(x[1]+x[0]*(x[3]-x[2])-0.5*4*(x[3]-x[2])*(x[3]-x[2])-(2*10-2*1.8*12*(1 - 0.15 * sin(0.2 * x[3]))+(100 + 12 * x[3] + 10 * cos(0.2 * x[3]) - 10)))
        #横截条件
        eqs.append(0.5*4*4+0.5*25+(-x[4]*x[3]+x[5])*4-(2*1.8*x[4]+x[4]*x[3]-x[5])*(-10 * cos(0.2 * x[3])))
        #协作变量
        eqs.append(-x[4]*x[2]+x[5]-3)

        t_1 = x[2],t_f = x[3],c_1 = x[4],c_2 = x[5]
        return t_1,t_f,c_1,c_2



    #求解控制序列为a_max、p时候的相关参数
    #t_1 = x[2]、t_f = x[3]、c_1 = x[4]、c_2 = x[5]
    def equation_7(x):
        eqs=[]
        #中间变量t1时刻的速度和位置
        eqs.append(x[0]-12-3*x[2])
        eqs.append(x[1]-30-12*x[2]-0.5*3*x[2]*x[2])
        #末态tf时刻的速度和位置
        eqs.append(x[0]+0.5*x[4]*(x[2]-x[3])*(x[2]-x[3])+0.5*x[5]-12*(1-0.15*sin(0.2*x[3])))
        eqs.append(x[1]+0.15*x[4]*(x[2]*x[2]*x[2]-x[2]*x[2]*x[2])+(0.5*x[4]*x[4]-x[5]*x[2])*(x[3]-x[2])-(2*10-2*1.8*12*(1 - 0.15 * sin(0.2 * x[3]))+(100 + 12 * x[3] + 10 * cos(0.2 * x[3]) - 10)))
        #横截条件
        eqs.append(0.5*(-x[4]*x[3]+x[5])*(-x[4]*x[3]+x[5])+0.5*25-(-x[4]*x[3]+x[5])*(-x[4]*x[3]+x[5])-(2*1.8*x[4]+x[4]*x[3]-x[5])*(-10 * cos(0.2 * x[3])))
        #协作变量
        eqs.append(-x[4]*x[2]+x[5]+4)

        t_1 = x[2],t_f = x[3],c_1 = x[4],c_2 = x[5]
        return t_1,t_f,c_1,c_2



    #求解控制序列为p时候的相关参数
    #t_f = x[0]、c_1 = x[1]、c_2 = x[2]
    def equation_8(x):
        eqs=[]
        eqs.append(12-0.5*x[1]*x[0]*x[0]+x[2]*x[0]-12*(1-0.15*sin(0.2*x[0])))
        eqs.append(30-0.15*x[1]*x[0]*x[0]*x[0]+0.5*x[2]*x[0]*x[0]+12*x[0]-(2*10-2*1.8*12*(1 - 0.15 * sin(0.2 * x[0]))+(100 + 12 * x[0] + 10 * cos(0.2 * x[0]) - 10)))

        t_f = x[0], c_1 = x[1],c_2 = x[2]
        return t_f,c_1,c_2



    #求解控制序列为a_min时候的相关参数
    # t_f = x[0]
    def equation_9(x):
        eqs = []
        # 末态tf时刻的速度和位置
        eqs.append(12 - 4 * x[0] - 12 * (1 - 0.15 * sin(0.2 * x[0])))
        eqs.append(30 + 12 * x[0] - 0.5 * 4 * x[0] * x[0] - (2 * 10 - 2 * 1.8 * 12 * (1 - 0.15 * sin(0.2 * x[0])) + (100 + 12 * x[0] + 10 * cos(0.2 * x[0]) - 10)))

        t_f = x[0]
        return t_f


    #判断控制序列为a_max、p、a_min时候的相关参数是否符合条件
    def condition_1(x):
        return


    #判断控序列为p、a_max时候的相关参数是否符合条件
    def condition_2(x):
        return

    #判断控制序列为a_min、p、a_max时候的相关参数是否符合条件
    def condition_3(x):
        return


    #判断控制序列为a_min、p时候的相关参数是否符合条件
    def condition_4(x):
        return

    #判断控制序列为a_max时候的相关参数是否符合条件
    def condition_5(x):
        return

    #判断控制序列为p、a_min时候的相关参数是否符合条件
    def condition_6(x):
        return

    #判断控制序列为a_max、p时候的相关参数是否符合条件
    def condition_7(x):
        return

    #判断控制序列为p时候的相关参数是否符合条件
    def condition_8(x):
        return

    # 判断控制序列为a_min时候的相关参数是否符合条件
    def condition_9(x):
        return


    equation_list = [equation_1, equation_2, equation_3,
                     equation_4, equation_5, equation_6,
                     equation_7, equation_8,equation_9]

    condition_list = [condition_1, condition_2, condition_3,
                      condition_4, condition_5, condition_6,
                      condition_7,condition_8, condition_9]

    initial_guess_1 = [0, 0, 0, 0, 0, 0, 0, 0, 0]
    initial_guess_2 = [0, 0, 0, 0, 0, 0]
    initial_guess_3 = [0, 0, 0, 0, 0, 0, 0, 0, 0]
    initial_guess_4 = [0, 0, 0, 0, 0, 0]
    initial_guess_5 = [0]
    initial_guess_6 = [0, 0, 0, 0, 0, 0]
    initial_guess_7 = [0, 0, 0, 0, 0, 0]
    initial_guess_8 = [0, 0, 0]
    initial_guess_9 = [0]

    initial_guess_list = [initial_guess_1, initial_guess_2, initial_guess_3, initial_guess_4, initial_guess_5,
                          initial_guess_6, initial_guess_7, initial_guess_8, initial_guess_9]
























                   




         
    
         


    























